package sensors;

import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.Semaphore;

import maze.Location;
import maze.Maze;
import robots.Ghost;
import robots.OwnGhost;
import simulation.Raycaster;
import simulation.Simulator;

public abstract class Pilot {

	protected OwnGhost ghost;
	protected Simulator simulator;
	Timer timer = new Timer();
	/**
	 * used to simulate travel & rotate
	 */
	public float speed = 350; // wheel-turnspeed of the ghosts, degree/s MUST BE THE
	// SAME AS THE SPEED OF THE REAL ROBOT!!
	private float wheelCirc = 15; // circumference of a wheel, cm MUST BE THE
	// SAME AS THE CIRC OF THE REAL ROBOT!!
	private float wheelBase = 15; // distance between robot wheels, cm MUST BE THE
	// SAME AS THE BASE OF THE REAL ROBOT!!
	abstract public void setSpeed(int speed);
	public Pilot(OwnGhost ghost,Simulator sim){
		this.ghost=ghost;
		simulator=sim;

	}
	public void simulateTravel(int distance){
		while (ghost.isTravelling);
		
		//System.out.println("acc: "+simulator.getGhostLocations().get(ghost));
		//System.out.println("acc rot: "+simulator.getGhostRotations().get(ghost));
		int period = 10; // milliseconds SHOULD BE SMALL ENOUGH (otherwise,
							// lightsensor will not read barcodes accurately)
		float travelSpeed = speed * wheelCirc / 360;

		TravelTask travelTask = new TravelTask(distance, travelSpeed, period);
		timer.schedule(travelTask, 0, period);
		try {
			simulator.getGhostLocks().get(ghost).acquire();// this blocks until the task is done
		} catch (InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}
	}
	
	abstract public int travelWithUltrasonicInterrupt(int distance, int margin);
	
 	public void simulateRotation(int angle) {
		/*
		 * try { ghostLocks.get(ghost).acquire(); } catch(InterruptedException
		 * ie) { throw new
		 * RuntimeException("The Simulator thread was interrupted."); }
		 */
		while (ghost.isTravelling)
			;
		int period = 10; // milliseconds
		float turnSpeed = (float) (((wheelCirc) * (speed)) / ((Math.PI) * (wheelBase)));

		RotateTask rotateTask = new RotateTask(angle, turnSpeed, period);
		timer.schedule(rotateTask, 0, period);
		try {
			simulator.getGhostLocks().get(ghost).acquire();
		} catch (InterruptedException ie) {
			throw new RuntimeException("The Simulator thread was interrupted.");
		}
	}
	
	class TravelTask extends TimerTask {

		private float dist;
		private int rot;
		private int remainingTicks;
		private int totalTicks;
		private float distancePerTick;

		/**
		 * Constructor
		 * 
		 * @param g
		 * @param distance
		 *            cm
		 * @param travelspeed
		 *            cm/s
		 * @param updateIntervalMilli
		 *            millisec
		 */
		public TravelTask(float distance, float travelspeed, float updateIntervalMilli) {
			rot = ghost.getRotation();
			dist = distance;

			distancePerTick = travelspeed * updateIntervalMilli / 1000;
			totalTicks = Math.abs((int) (distance / distancePerTick)); // =
																		// totaltime/timepertick
			remainingTicks = totalTicks;
			ghost.isTravelling = true;

		}

		@Override
		public void run() {
			rot = simulator.getGhostRotations().get(ghost);
			if (remainingTicks > 0) {

				int neg = 1; // -1 if negative distance +1 if positive
				if (dist < 0) neg = -1;
				Location prev = simulator.getGhostLocations().get(ghost);
				double newX;
				double newY;
				double rotRad = Math.toRadians(rot);
				newX = prev.x + neg * (Math.cos(rotRad) * distancePerTick);
				newY = prev.y + neg * (Math.sin(rotRad) * distancePerTick);
				
				Location newLoc = new Location(newX, newY);
				simulator.getGhostLocations().put(ghost, newLoc);
				Maze maze=simulator.getMaze();
				Location location=simulator.getGhostLocations().get(ghost);
				if (dist < 0 && (float) Raycaster.cast(maze, location, rot + 180) <= 2 * distancePerTick) {

					rotRad = Math.toRadians(rot);
					newX = newX + neg * (Math.cos(rotRad) * (float) (Raycaster.cast(maze, location, rot + 180)) - 0.1);
					newY = newY + neg * (Math.sin(rotRad) * (float) (Raycaster.cast(maze, location, rot + 180)) - 0.1);

					newLoc = new Location(newX, newY);
					simulator.getGhostLocations().put(ghost, newLoc);
					simulator.getGhostLocks().get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				if (dist > 0 && (float) Raycaster.cast(maze, location, rot) <= 2 * distancePerTick) {
					rotRad = Math.toRadians(rot);
					newX = newX + neg * (Math.cos(rotRad) * (float) (Raycaster.cast(maze, location, rot)) - 0.1);
					newY = newY + neg * (Math.sin(rotRad) * (float) (Raycaster.cast(maze, location, rot)) - 0.1);

					newLoc = new Location(newX, newY);
					simulator.getGhostLocks().get(ghost).release();
					ghost.isTravelling = false;
					this.cancel();
				}
				remainingTicks--;
			} else {
				/*
				 * int neg = 1; // -1 if negative distance +1 if positive if
				 * (dist<0) neg = -1; //set final pos Location prev =
				 * ghostLocations.get(ghost); double newX; double newY; double
				 * rotRad = Math.toRadians(rot); newX = prev.x +
				 * neg*(Math.cos(rotRad
				 * )*distancePerTick);//(float)(maze.getMaxDistance
				 * (ghostLocations.get(ghost), rot))-0.1); newY = prev.x +
				 * neg*(Math
				 * .sin(rotRad)*distancePerTick);//(float)(maze.getMaxDistance
				 * (ghostLocations.get(ghost), rot))-0.1);
				 * 
				 * Location newLoc = new Location(newX, newY);
				 * ghostLocations.put(ghost, newLoc);
				 */
				//Location prev = simulator.getGhostLocations().get(ghost);
				//Location newLoc=new Location(Math.round(prev.getX()),Math.round(prev.getY()));
				//simulator.getGhostLocations().put(ghost, newLoc);
				ghost.isTravelling = false;
				simulator.getGhostLocks().get(ghost).release();
				
				this.cancel();
			}

		}
		
	}
	class RotateTask extends TimerTask {

		private int angle;
		private int startAngle;
		private int remainingTicks;
		private int totalTicks;
		private float degreesPerTick;

		/**
		 * Constructor
		 * 
		 * @param g
		 * @param angle
		 *            degrees to turn CCW
		 * @param turnspeed
		 *            degrees/s
		 * @param updateIntervalMilli
		 *            milliseconds
		 */
		public RotateTask(int angle, double turnspeed, int updateIntervalMilli) {
			startAngle = simulator.getGhostRotations().get(ghost);
			this.angle = angle;

			degreesPerTick = (float) (turnspeed * (float) (updateIntervalMilli) / 1000.0);
			totalTicks = (int) (Math.abs(angle) / degreesPerTick);
			remainingTicks = totalTicks;
		}

		@Override
		public void run() {
			ghost.isTravelling = true;
			if (remainingTicks > 0) {
				int newAngle = (int) (startAngle + Math.signum(angle) * degreesPerTick * (totalTicks - remainingTicks));
				simulator.getGhostRotations().put(ghost, newAngle % 360);
				remainingTicks--;
			} else {
				simulator.getGhostRotations().put(ghost,(startAngle + angle) % 360);
				ghost.isTravelling = false;
				simulator.getGhostLocks().get(ghost).release();
				this.cancel();
			}
		}
	}

}
